#include "CameraController.hpp"
#include "CameraFormatUtils.hpp"
#include "cbb_log_api.h"
#include "iostream"
#include "make_unique.hpp"
CameraController::CameraController() : m_isCapturing(false) {}

CameraController::~CameraController() {
    stopCapture();
}

void CameraController::initCamera(const MultiCameraConfig &config) {
    m_devices.clear();
    for (const auto &cameraConfig : config.cameras) {
        if (!cameraConfig.baseConfig.enable) {
            continue; // 跳过未启用的摄像头
        }

        V4L2CameraConfig v4l2Config = convertToV4L2CameraConfig(cameraConfig);
        auto camera = std::make_unique<V4L2Camera>(v4l2Config);
        if (camera->open()) {
            m_devices.push_back(std::move(camera));
            CbbLogInfo("Camera %s opened successfully", cameraConfig.baseConfig.deviceName.c_str());
            std::cout << "Camera  opened successfully" << cameraConfig.baseConfig.deviceName.c_str() << std::endl;
        } else {
            CbbLogError("Failed to open camera %s", cameraConfig.baseConfig.deviceName.c_str());
        }
    }
}

void CameraController::startCapture(std::function<void(int, const CbbCameraFrame_t &)> frameCallback) {
    m_frameCallback = std::move(frameCallback);
    m_isCapturing = true;

    for (size_t i = 0; i < m_devices.size(); ++i) {
        if (V4L2Camera *camera = getCamera(i)) {
            if (camera->startCapture()) {
                m_captureThreads.emplace_back(&CameraController::captureLoop, this, i);
                std::cout << "Started capturing for camera " << i << std::endl;
            } else {
                std::cerr << "Failed to start capture for camera " << i << std::endl;
            }
        }
    }
}

void CameraController::stopCapture() {
    m_isCapturing = false;
    for (auto &thread : m_captureThreads) {
        if (thread.joinable()) {
            thread.join();
        }
    }
    m_captureThreads.clear();
    for (auto &device : m_devices) {
        device->stopCapture();
        device->close();
    }
}

void CameraController::captureLoop(int cameraId) {
    V4L2Camera *camera = getCamera(cameraId);
    if (!camera)
        return;

    while (m_isCapturing) {
        CbbCameraFrame_t frame;
        if (camera->captureFrame(frame, 1000)) { // 1000ms timeout
            if (m_frameCallback) {
                m_frameCallback(cameraId, frame);
            }
            camera->releaseFrame(frame);
        } else {
            std::cerr << "Failed to capture frame from camera " << cameraId << std::endl;
        }
    }
}

bool CameraController::updateCameraConfig(int cameraId, const CameraConfig &config) {
    V4L2Camera *camera = getCamera(cameraId);
    if (!camera) {
        CbbLogError("Invalid camera ID: %d", cameraId);
        return false;
    }
    V4L2CameraConfig v4l2Config = convertToV4L2CameraConfig(config);
    camera->updateConfig(v4l2Config);

    CbbLogInfo("Updating configuration for camera %d", cameraId);
    return true;
}

V4L2CameraConfig CameraController::convertToV4L2CameraConfig(const CameraConfig &config) {
    V4L2CameraConfig v4l2Config;
    v4l2Config.deviceName = config.baseConfig.deviceName;
    v4l2Config.width = config.baseConfig.width;
    v4l2Config.height = config.baseConfig.height;
    v4l2Config.fps = config.baseConfig.fps;

    uint32_t formatValue;
    if (CameraFormatUtils::stringToFormat(config.baseConfig.format, formatValue)) {
        v4l2Config.format = formatValue;
    } else {
        CbbLogError("Failed to convert format string: %s", config.baseConfig.format.c_str());
        v4l2Config.format = V4L2_PIX_FMT_YUYV; // 默认格式
    }
    return v4l2Config;
}
V4L2Camera *CameraController::getCamera(int cameraId) {
    if (cameraId >= 0 && cameraId < static_cast<int>(m_devices.size())) {
        return m_devices[cameraId].get();
    }
    return nullptr;
}